/*
 *  fix.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 26/08/2011
 *
 */

#include "fix.h"

Fix::Fix()
:	Constraint()
{}

// ***

Fix::Fix(vector<string>& vp, int fdn, int di, double et)
:	Constraint(vp,di),
	fixedP(),
	endTime(et),
	fixedofNumber(fdn)
{
	isQuadratic = true;
}

// ***

Fix::Fix(vector<string>& vp, int fdn, int di, double et, double pen)
:	Constraint(vp,di),
	fixedP(),
	endTime(et),
	fixedofNumber(fdn)
{
	isQuadratic = true;
	penalty = pen;
}

// ***

void Fix::calcPhi(const VectorXd & Q)
{
	if (fixedP.size()==0)
	{
		fillFixedP();
	}
	//cout << "time = " << time << endl;
	//cout << "endTime = " << endTime << endl;
	if (time<=endTime || endTime==0.0)
	{
		Phi = Q(fixedofNumber-1)-fixedP(fixedofNumber-1);
	}
	else Phi = 0.0;
}

// ***

void Fix::calcDPhi(const VectorXd & Q)
{
	dPhi.setZero();
	if (time<=endTime || endTime==0.0)
	{
		dPhi(fixedofNumber-1) = 1.0;
	}
}

// ***

void Fix::calcDDPhi(const VectorXd & Q)
{
	ddPhi.setZero();
}

// ***

void Fix::setFqSymmetries()
{}

// ***

void Fix::setKqSymmetries()
{}

// ***

void Fix::fillFixedP()
{
	fixedP = q;
}